/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h"

#include <utility>
#include <vector>

#include "Eigen/Core"
#include "cartographer/common/internal/ceres_solver_options.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace mapping
    {
        namespace scan_matching
        {

            proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D(
                common::LuaParameterDictionary *const parameter_dictionary)
            {
                proto::CeresScanMatcherOptions2D options;
                options.set_occupied_space_weight(
                    parameter_dictionary->GetDouble("occupied_space_weight"));
                options.set_translation_weight(
                    parameter_dictionary->GetDouble("translation_weight"));
                options.set_rotation_weight(
                    parameter_dictionary->GetDouble("rotation_weight"));
                *options.mutable_ceres_solver_options() = common::CreateCeresSolverOptionsProto(
                    parameter_dictionary->GetDictionary("ceres_solver_options").get());
                return options;
            }

            // 利用ceres自动求导，所以cartographer不用关注求导问题
            CeresScanMatcher2D::CeresScanMatcher2D(const proto::CeresScanMatcherOptions2D &options)
                : options_(options),
                  ceres_solver_options_(common::CreateCeresSolverOptions(options.ceres_solver_options()))
            {
                ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
            }

            CeresScanMatcher2D::~CeresScanMatcher2D()
            {
            }

            /// @brief 在[x,y,yaw]三个维度迭代优化
            /// @param target_translation
            /// @param initial_pose_estimate
            /// @param point_cloud
            /// @param grid
            /// @param pose_estimate
            /// @param summary
            void CeresScanMatcher2D::Match(const Eigen::Vector2d &target_translation,
                                           const transform::Rigid2d &initial_pose_estimate,
                                           const sensor::PointCloud &point_cloud,
                                           const Grid2D &grid,
                                           transform::Rigid2d *const pose_estimate,
                                           ceres::Solver::Summary *const summary) const
            {
                double occupied_weight = options_.occupied_space_weight();
                CHECK_GT(occupied_weight, 0.);
                double size_sqrt = std::sqrt(static_cast<double>(point_cloud.size()));
                occupied_weight /= size_sqrt;
                double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
                                                 initial_pose_estimate.translation().y(),
                                                 initial_pose_estimate.rotation().angle()};
                ceres::Problem problem;
                // 地图残差是hit点在栅格地图中的概率，迭代的坐标要使得整个点云的占据概率最大
                switch (grid.GetGridType())
                {
                case GridType::PROBABILITY_GRID: {
                    auto *cost_func = CreateOccupiedSpaceCostFunction2D(occupied_weight, point_cloud, grid);
                    problem.AddResidualBlock(cost_func, nullptr /* loss function */, ceres_pose_estimate);
                    break;
                }
                case GridType::TSDF: {
                    auto *cost_func = CreateTSDFMatchCostFunction2D(occupied_weight, point_cloud, static_cast<const TSDF2D &>(grid));
                    problem.AddResidualBlock(cost_func, nullptr /* loss function */, ceres_pose_estimate);
                    break;
                }
                }
                // AddResidualBlock的最后一个参数是优化目标
                // 平移残差,两个维度(即x和y的误差)
                CHECK_GT(options_.translation_weight(), 0.);
                auto *cost_translation = TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(options_.translation_weight(), target_translation);
                problem.AddResidualBlock(cost_translation, nullptr /* loss function */, ceres_pose_estimate);
                // 旋转残差,一个维度(即yaw)
                CHECK_GT(options_.rotation_weight(), 0.);
                auto *cost_rotate = RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(options_.rotation_weight(), ceres_pose_estimate[2]);
                problem.AddResidualBlock(cost_rotate, nullptr /* loss function */, ceres_pose_estimate);
                ceres::Solve(ceres_solver_options_, &problem, summary);
                // 获取优化结果
                *pose_estimate = transform::Rigid2d({ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
            }

        } // namespace scan_matching
    }     // namespace mapping
} // namespace cartographer
